//!!!!! 140, 120 deg and 90 deg swash config implimented but no Rudder nor Throttle control
//!!!!! added corrections for roll lever arm in SWASH configurations
//!!!!! zeroed boost terms for fast return to manual from Auto mode
//!!!!! added code to calculate collective term from Tx inputs and subtract from UDB outputs
//!!!!! added collective correction for all 3 swash configurations
//!!!!! fixed reversing code for helis

#include "p30f4011.h"
#include "definesRmat.h"
#include "defines.h"

//	Perform control based on the airframe type.
//	Use the radio to determine the baseline pulse widths if the radio is on.
//	Otherwise, use the trim pulse width measured during power up.
//
//	Mix computed roll and pitch controls into the output channels for the compiled airframe type


int aileronbgain = (int)(8.0*AILERON_BOOST) ;
int elevatorbgain = (int)(8.0*ELEVATOR_BOOST) ;
int rudderbgain = (int)(8.0*RUDDER_BOOST) ;

int pwManual[MAX_INPUTS+1] ;


void servoMix( void )
{
	long temp ;


	// If radio is off, use pwTrim values instead of the pwIn values	
	for (temp = 1; temp <= NUM_INPUTS; temp++)
		if (flags._.radio_on)
			pwManual[temp] = pwIn[temp];
		else
			pwManual[temp] = pwTrim[temp];
	
	
	// Apply boosts if in a stabilized mode
	//!!!!! Also check here for fast return to manual to zero boost terms
	//!!!!! Also apply collective correction if a heli
	//!!!!! We do this unless radio is off (if no radio on, no boost or collective corrections needed)

	if (flags._.radio_on && flags._.pitch_feedback && (pwIn[MODE_SWITCH_INPUT_CHANNEL] > MODE_SWITCH_THRESHOLD_LOW))
	{
//!!!!! add collective calculation only if Heli airframe
#if ( AIRFRAME_TYPE == AIRFRAME_HELI )
#define const1 21845 // 120 deg collective denominator term
#define const2 32768 // 90 deg collective denominator term
#define const3 42795 // 140 deg collective denominator term

		//!!!!! See Tech Note on collective correction and how to determine signs and diagnose effect
		//!!!!! get collective term 
		getCollective() ;

		// General form of pwManual is raw Tx plus raw Tx corrected for trims and collective times gain		  

		pwManual[AILERON_INPUT_CHANNEL] += ((pwManual[AILERON_INPUT_CHANNEL] - pwTrim[AILERON_INPUT_CHANNEL] + collective) * aileronbgain) >> 3 ;
		pwManual[AILERON_SECONDARY_INPUT_CHANNEL] += ((pwManual[AILERON_SECONDARY_INPUT_CHANNEL] - pwTrim[AILERON_SECONDARY_INPUT_CHANNEL] - collective) * aileronbgain) >> 3 ;
		pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] - pwTrim[ELEVATOR_INPUT_CHANNEL] + collective) * elevatorbgain) >> 3 ;

	//!!!!! no Rudder control yet
	//	pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] - pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3 ;
#else // Normal airplane code

		pwManual[AILERON_INPUT_CHANNEL] += ((pwManual[AILERON_INPUT_CHANNEL] - pwTrim[AILERON_INPUT_CHANNEL]) * aileronbgain) >> 3 ;
		pwManual[ELEVATOR_INPUT_CHANNEL] += ((pwManual[ELEVATOR_INPUT_CHANNEL] - pwTrim[ELEVATOR_INPUT_CHANNEL]) * elevatorbgain) >> 3 ;
		pwManual[RUDDER_INPUT_CHANNEL] += ((pwManual[RUDDER_INPUT_CHANNEL] - pwTrim[RUDDER_INPUT_CHANNEL]) * rudderbgain) >> 3 ;

#endif
	}

	
	// Standard airplane airframe
	// Mix roll_control into ailerons
	// Mix pitch_control into elevators
	// Mix yaw control and waggle into rudder
#if ( AIRFRAME_TYPE == AIRFRAME_STANDARD )
		temp = pwManual[AILERON_INPUT_CHANNEL] + REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle) ;
		pwOut[AILERON_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
			REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED, pwOut[AILERON_OUTPUT_CHANNEL] - 3000) ;
		
		temp = pwManual[ELEVATOR_INPUT_CHANNEL] + REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control) ;
		pwOut[ELEVATOR_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		temp = pwManual[RUDDER_INPUT_CHANNEL] + REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control - waggle) ;
		pwOut[RUDDER_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		
		if ( pwManual[THROTTLE_INPUT_CHANNEL] == 0 )
		{
			pwOut[THROTTLE_OUTPUT_CHANNEL] = 0 ;
		}
		else
		{	
			temp = pwManual[THROTTLE_INPUT_CHANNEL] + REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, altitude_control) ;
			if ( THROTTLE_CHANNEL_REVERSED )
			{
				if (temp > pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			else
			{
				if (temp < pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			pwOut[THROTTLE_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		}
#endif
	
	
	// V-Tail airplane airframe
	// Mix roll_control and waggle into ailerons
	// Mix pitch_control and yaw_control into both elevator and rudder
#if ( AIRFRAME_TYPE == AIRFRAME_VTAIL )
		yaw_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, yaw_control) ;
		
		temp = pwManual[AILERON_INPUT_CHANNEL] + REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_control + waggle) ;
		pwOut[AILERON_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		//	Reverse the polarity of the secondary aileron if necessary
		pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = 3000 +
			REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED, pwOut[AILERON_OUTPUT_CHANNEL] - 3000) ;
		
		temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
			REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control + yaw_control) ;
		pwOut[ELEVATOR_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		temp = pwManual[RUDDER_INPUT_CHANNEL] +
			REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, pitch_control - yaw_control) ;
		pwOut[RUDDER_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		if ( pwManual[THROTTLE_INPUT_CHANNEL] == 0 )
		{
			pwOut[THROTTLE_OUTPUT_CHANNEL] = 0 ;
		}
		else
		{	
			temp = pwManual[THROTTLE_INPUT_CHANNEL] + REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, altitude_control) ;
			if ( THROTTLE_CHANNEL_REVERSED )
			{
				if (temp > pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			else
			{
				if (temp < pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			pwOut[THROTTLE_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		}
#endif
	
	
	// Delta-Wing airplane airframe
	// Mix roll_control, pitch_control, and waggle into aileron and elevator
	// Mix rudder_control into  rudder
#if ( AIRFRAME_TYPE == AIRFRAME_DELTA )
		roll_control = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, roll_control) ;
		
		temp = pwManual[AILERON_INPUT_CHANNEL] +
			REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, -roll_control + pitch_control - waggle) ;
		pwOut[AILERON_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		temp = pwManual[ELEVATOR_INPUT_CHANNEL] +
			REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, roll_control + pitch_control + waggle) ;
		pwOut[ELEVATOR_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		temp = pwManual[RUDDER_INPUT_CHANNEL] + REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control) ;
		pwOut[RUDDER_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		
		if ( pwManual[THROTTLE_INPUT_CHANNEL] == 0 )
		{
			pwOut[THROTTLE_OUTPUT_CHANNEL] = 0 ;
		}
		else
		{	
			temp = pwManual[THROTTLE_INPUT_CHANNEL] + REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, altitude_control) ;
			if ( THROTTLE_CHANNEL_REVERSED )
			{
				if (temp > pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			else
			{
				if (temp < pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			pwOut[THROTTLE_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		}
#endif
	
	
	// Helicopter airframe
	// Ignore waggle for now

#if ( AIRFRAME_TYPE == AIRFRAME_HELI )
//!!!!! may want to recode to use __builtin_mulsu or mulss

//!!!!! Calculate inputs to roll serrvos depending on swash config
//!!!!! pitch servo always gets full pitch_control

		union longww roll_corrected  ;						// roll control correction for roll lever arm
		union longww pitch_corrected ;						// pitch control correction for pitch lever arm

#if (SWASH_TYPE == SWASH1)								  	// 120 deg swash

		roll_corrected.WW = ((long)roll_control*(long)28378)<<1 ;	  	// mult by cos(30) = 0.867
		pitch_corrected._.W1 = pitch_control>>1 ;						// mult by sin(30) = 0.5

#elif (SWASH_TYPE == SWASH2)							  	// 90 deg swash

//!!!!! had it wrong...there is no pitch input to roll for 90 deg, so this should be 0
		roll_corrected._.W1 = roll_control ;	  			// no correction
//		pitch_corrected._.W1 = pitch_control ;				// no correction
		pitch_corrected._.W1 = 0 ;							// no pitch input to roll

#elif (SWASH_TYPE == SWASH3)
							  	// 140 deg swash 
		roll_corrected.WW = ((long)roll_control*(long)21063)<<1 ;	// multiply by cos(50) = 0.643
		pitch_corrected.WW = ((long)pitch_control*(long)25101)<<1 ;	// multiply by sin(50) = 0.766

#endif

//!!!!! Now calculate 3 swash servo outputs

		temp = pwManual[AILERON_INPUT_CHANNEL] +
			REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, roll_corrected._.W1 + waggle) +
			REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED,-pitch_corrected._.W1) ;
		pwOut[AILERON_OUTPUT_CHANNEL] = pulsesat( temp ) ;

		temp = pwManual[ELEVATOR_INPUT_CHANNEL] + 
			REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_control) ;
		pwOut[ELEVATOR_OUTPUT_CHANNEL] = pulsesat( temp ) ;
		
		temp = pwManual[AILERON_SECONDARY_INPUT_CHANNEL] + 
			REVERSE_IF_NEEDED(AILERON_SECONDARY_CHANNEL_REVERSED, roll_corrected._.W1 + waggle) + 
			REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, pitch_corrected._.W1) ;
		pwOut[AILERON_SECONDARY_OUTPUT_CHANNEL] = pulsesat( temp ) ;



	//!!!!! need to include new HH code here when ready
	//	temp = pwManual[RUDDER_INPUT_CHANNEL] /*+ REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, yaw_control)*/ ;
	//	pwOut[RUDDER_OUTPUT_CHANNEL] = pulsesat( temp ) ;

	//!!!!! we will use manual throttle control until we get a better altitude sensor
	//!!!!! also throttle control is more complex for helis that just speed
	//!!!!! depends on the attitude of the heli and if we want altitude and/or speed in some direction
	//!!!!! comment out for now

/*		
		if ( pwManual[THROTTLE_INPUT_CHANNEL] == 0 )
		{
			pwOut[THROTTLE_OUTPUT_CHANNEL] = 0 ;
		}

		else
		{	
			temp = pwManual[THROTTLE_INPUT_CHANNEL] + REVERSE_IF_NEEDED(THROTTLE_CHANNEL_REVERSED, altitude_control) ;
			if ( THROTTLE_CHANNEL_REVERSED )
			{
				if (temp > pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			else
			{
				if (temp < pwTrim[THROTTLE_INPUT_CHANNEL]) temp = pwTrim[THROTTLE_INPUT_CHANNEL] ;
			}
			pwOut[THROTTLE_OUTPUT_CHANNEL] =  pulsesat( temp ) ;
		}
*/
#endif
}
